#include "./PORT/port.h"
#include "../System/config.h"

#include "./BSP/TRACK/track.h"

static const float weights[8] = {-6.0f, -4.0f, -2.0f, -1.0f, 1.0f, 2.0f, 4.0f, 6.0f}; // 8个灰度值对应的权值

static uint8_t digital = 0x00;

/**
 * @brief       初始化循迹传感器
 * @param       无
 * @retval      无
 */
void track_init(void)
{
	i2c_init(CFG_TRACK_IIC_ID, CFG_TRACK_IIC_SDA, CFG_TRACK_IIC_SCL);
}

/**
 * @brief       读取循迹传感器数据
 * @param       无
 * @retval      无
 */
void track_read_sensors(void)
{
	uint8_t data;
	i2c_read_data(CFG_TRACK_IIC_ID, GW_GRAY_ADDR_DEF << 1, GW_GRAY_DIGITAL_MODE, &data, 1); // 读取传感器数据
	digital = ~data;																		// 取反数据，0表示黑线，1表示白线
}

/**
 * @brief       获取传感器数据
 * @param       无
 * @retval      传感器数据
 */
uint8_t track_get_sensor_data(void)
{
	return digital; // 返回传感器数据
}

/**
 * @brief       获取循迹误差值
 * @param        sensor_data: 传感器数据
 * @retval      循迹误差值
 */
float track_get_line_position_error(uint8_t sensor_data)
{
	float error = 0.0f;
	uint8_t black_count = 0;
	for (uint8_t i = 0; i < 8; i++)
	{
		if (sensor_data & (1 << i))
		{
			error += weights[i];
			black_count++;
		}
	}
	if (black_count > 0)
	{
		error /= black_count; // 计算平均误差
	}
	return error; // 返回循迹误差值
}

/**
 * @brief       显示循迹传感器数据
 * @param       无
 * @retval      无
 */
void track_oled_show(void)
{
#include "./BSP/OLED/OLED.h"
	uint8_t cnt = track_get_sensor_data();
	OLED_Printf(0, 8 * 5, OLED_6X8, "BIT:%d %d %d %d %d %d %d %d        ", cnt & 0x01 ? 1 : 0, cnt & 0x02 ? 1 : 0, cnt & 0x04 ? 1 : 0, cnt & 0x08 ? 1 : 0, cnt & 0x10 ? 1 : 0, cnt & 0x20 ? 1 : 0, cnt & 0x40 ? 1 : 0, cnt & 0x80 ? 1 : 0);
}
